import numpy as np
from scipy.optimize import least_squares


def calculate_label_coordinates(*distances):
    """
    计算标签的坐标，输入为 s0, s1 到 s3 的距离，使用最小二乘法减少误差
    """

    def distance_equations(vars, *distances):
        x, y = vars
        equations = []
        # 假设 d0 为原点 (0, 0)，d1 为 (1, 0)，d2 为 (1, 1)，d3 为 (0, 1) 作为示例，可根据实际情况修改
        points = [(0, 0), (2.4, 0), (2.4, 2.4), (0, 2.4)]
        for i, dist in enumerate(distances):
            dx = points[i][0] - x
            dy = points[i][1] - y
            equations.append(np.sqrt(dx ** 2 + dy ** 2) - dist)
        return equations

    # 初始猜测，可根据实际情况调整
    x0 = 0.5
    y0 = 0.5
    result = least_squares(distance_equations, [x0, y0], args=distances)
    x, y = result.x
    return x, y


# 示例使用
if __name__ == "__main__":
    # # 输入至少 s0, s1 两个距离，至多 s0~s3 四个距离
    # # 示例 1: 输入两个距离 s0 和 s1
    # x1, y1 = calculate_label_coordinates(2, 2)
    # print(f"Coordinates with s0=1.0 and s1=1.5: ({x1}, {y1})")

    # 示例 2: 输入三个距离 s0, s1 和 s2
    x2, y2 = calculate_label_coordinates(2.02, 3.05, 2.69)
    print(f"Coordinates with s0=1.0, s1=1.5, s2=2.0: ({x2}, {y2})")

    # # 示例 3: 输入四个距离 s0, s1, s2 和 s3
    # x3, y3 = calculate_label_coordinates(1.0, 1.5, 2.0, 2.5)
    # print(f"Coordinates with s0=1.0, s1=1.5, s2=2.0, s3=2.5: ({x3}, {y3})")